import os.path as path
from datetime import datetime

import matplotlib.pyplot as plt
import prettytable as pt
from PyQt5 import QtGui

from PyQt5.QtCore import QThread, pyqtSignal, Qt, QPoint
from PyQt5.QtGui import QCloseEvent

from PyQt5.QtWidgets import QMainWindow, QFileDialog, QMenu, QApplication

from Algorithm.ErrorAnalyser import ErrorAnalyser, Error
from ui_error_analyse import Ui_Error_Analyse


class ErrorAnalyserThread(QThread):
    finishSignal = pyqtSignal(ErrorAnalyser)

    def __init__(self, target_file, ref_file, column_index,
                 delimiter='\s+',
                 skip_rows=0,
                 cdf_dense=None,
                 write2file=False,
                 offset=None,
                 error_hz=5):
        super(ErrorAnalyserThread, self).__init__()
        self.target_file = target_file
        self.ref_file = ref_file
        self.column_index = column_index
        self.delimiter = delimiter
        self.skip_rows = skip_rows
        self.cdf_dense = cdf_dense
        self.error_hz = error_hz
        self.write2file = write2file
        self.thread = None
        self.offset = offset

    def run(self) -> None:
        e = ErrorAnalyser(self.target_file, self.ref_file, self.column_index,
                          skip_rows=self.skip_rows,
                          delimiter=self.delimiter,
                          error_hz=self.error_hz,
                          write2file=self.write2file,
                          offset=self.offset,
                          bins=self.cdf_dense)
        e.analyse(True, True)
        self.finishSignal.emit(e)


class ErrorAnalyseWindow(QMainWindow, Ui_Error_Analyse):
    def __init__(self):
        super(ErrorAnalyseWindow, self).__init__()
        self.setupUi(self)
        self.btnBrowseTargetFile.clicked.connect(self.onBtnBrowserTargetFile)
        self.btnBrowseRefFile.clicked.connect(self.onBtnBrowserRefFile)
        self.btnCalculateError.clicked.connect(self.onBtnStart)
        self.btnClosePlot.clicked.connect(self.onBtnClosePlot)
        self.btnClearLog.clicked.connect(self.clear_log)
        self.option = {}
        self.thread = None
        self.column_index = None
        self.set_gui()
        self.listLog.customContextMenuRequested[QPoint].connect(self.__right_menu_on_log)

    def set_gui(self):
        self.option['column_index'] = self.treeWidget.topLevelItem(0)
        self.option['gps_week_index'] = self.treeWidget.topLevelItem(0).child(0)
        self.option['gps_second_index'] = self.treeWidget.topLevelItem(0).child(1)
        self.option['latitude_index'] = self.treeWidget.topLevelItem(0).child(2)
        self.option['longitude_index'] = self.treeWidget.topLevelItem(0).child(3)
        self.option['height_index'] = self.treeWidget.topLevelItem(0).child(4)
        self.option['vn_index'] = self.treeWidget.topLevelItem(0).child(5)
        self.option['ve_index'] = self.treeWidget.topLevelItem(0).child(6)
        self.option['vd_index'] = self.treeWidget.topLevelItem(0).child(7)
        self.option['roll_index'] = self.treeWidget.topLevelItem(0).child(8)
        self.option['pitch_index'] = self.treeWidget.topLevelItem(0).child(9)
        self.option['yaw_index'] = self.treeWidget.topLevelItem(0).child(10)
        self.option['q_index'] = self.treeWidget.topLevelItem(0).child(11)
        self.option['ns_index'] = self.treeWidget.topLevelItem(0).child(12)
        self.option['error_hz'] = self.treeWidget.topLevelItem(1)
        self.option['skip_rows'] = self.treeWidget.topLevelItem(2)
        self.option['cdf_dense'] = self.treeWidget.topLevelItem(3)
        self.option['write2file'] = self.treeWidget.topLevelItem(4)
        self.option['offset_check'] = self.treeWidget.topLevelItem(5)
        self.option['offset'] = [self.treeWidget.topLevelItem(5).child(0),
                                 self.treeWidget.topLevelItem(5).child(1),
                                 self.treeWidget.topLevelItem(5).child(2)]

    def __right_menu_on_log(self, point):
        popMenu = QMenu()
        copy_action = popMenu.addAction(u'Copy')
        copy_action.triggered.connect(lambda: self.__copy_log(point))
        clear_action = popMenu.addAction(u'Clear')
        clear_action.triggered.connect(self.clear_log)
        popMenu.exec_(QtGui.QCursor.pos())

    def __copy_log(self, point):
        item = self.listLog.itemAt(point)
        if not item is None:
            clipboard = QApplication.clipboard()
            clipboard.setText(item.text())
            self.show_log("copy to clip board")

    def onBtnStart(self):
        self.show_log("start error analysing...")
        target_file = self.editTargetFile.text()
        ref_file = self.editRefFile.text()

        if path.isdir(target_file):
            self.show_log("target is not a file:\n%s" % target_file)
            return
        if path.isdir(ref_file):
            self.show_log("ref file is not a file:\n%s" % ref_file)
            return
        try:
            cdf_dense = None
            error_hz = int(self.option['error_hz'].text(1))
            skip_rows = int(self.option['skip_rows'].text(1))
            if self.option['cdf_dense'].checkState(1) == Qt.Checked:
                cdf_dense = int(self.option['cdf_dense'].text(1))
        except Exception as e:
            error_hz = 5
            skip_rows = 0
            cdf_dense = None
            self.show_log(str(e.args))
        try:
            column_index = [
                int(self.option['gps_week_index'].text(1)),
                int(self.option['gps_second_index'].text(1)),
                int(self.option['latitude_index'].text(1)),
                int(self.option['longitude_index'].text(1)),
                int(self.option['height_index'].text(1)),
                int(self.option['vn_index'].text(1)),
                int(self.option['ve_index'].text(1)),
                int(self.option['vd_index'].text(1)),
                int(self.option['roll_index'].text(1)),
                int(self.option['pitch_index'].text(1)),
                int(self.option['yaw_index'].text(1)),
                int(self.option['q_index'].text(1)),
                int(self.option['ns_index'].text(1)),
            ]
            self.column_index = column_index

        except Exception as e:
            self.column_index = [0, 1, 2, 3, 4, -1, -1, -1, -1, -1, -1, -1, -1]
            self.show_log(str(e.args))

        write2file = self.option['write2file'].checkState(1) == Qt.Checked
        offset = None
        if self.option['offset_check'].checkState(1) == Qt.Checked:

            offset = [float(t.text(1)) for t in self.option['offset']]
            print("offset enable")
        self.thread = ErrorAnalyserThread(target_file, ref_file, self.column_index,
                                          skip_rows=skip_rows,
                                          cdf_dense=cdf_dense,
                                          write2file=write2file,
                                          error_hz=error_hz,
                                          offset=offset
                                          )

        self.thread.finishSignal.connect(self.onErrorAnalyseFinished)
        self.thread.start()
        self.btnCalculateError.setEnabled(False)

    def onBtnClosePlot(self):
        self.show_log('close plot')
        plt.close('all')

    def onErrorAnalyseFinished(self, e: ErrorAnalyser):
        self.btnCalculateError.setEnabled(True)
        self.show_log("error analyse finished")
        if e.exception is not None:
            self.show_log(str(e.exception))
            return
        # self.showRMS(e)
        plt.close('all')
        self.__plot_position_error(e)
        self.__plot_vel_error(e)
        self.__plot_atti_error(e)
        self.__plot_tables(e)
        self.__plot_2d_error(e)
        self.__plot_3d_error(e)
        plt.show()

    def __plot_position_error(self, e: ErrorAnalyser):
        if self.column_index[2] > 0 and self.column_index[3] > 0 and self.column_index[4] > 0:
            ylabels = ["East", "North", "Up"]
            plt.figure("pos_err")
            plt.subplot(3, 1, 1)
            plt.ylabel(ylabels[0])
            plt.title("position error")
            plt.plot(e.times - e.times[0], e.errs[self.column_index[2]].error, 'g')
            plt.legend(
                ['East error(rms = %5.3f)' % e.errs[self.column_index[2]].rms])  # err_rms[self.column_index[2]]])

            plt.subplot(3, 1, 2)
            plt.ylabel(ylabels[1])
            plt.plot(e.times - e.times[0], e.errs[self.column_index[3]].error, 'g')
            plt.legend(['North error(rms = %5.3f)' % e.errs[self.column_index[3]].rms])
            plt.subplot(3, 1, 3)
            plt.ylabel(ylabels[2])
            plt.plot(e.times - e.times[0], e.errs[self.column_index[4]].error, 'g')
            plt.legend(['Down error(rms = %5.3f)' % e.errs[self.column_index[4]].rms])
            plt.xlabel("gps time/s - %f" % e.times[0])
            ''' plot cdf and pdf'''
            plt.figure('pos_pdf_cdf')
            plt.title("position error pdf and cdf")

            _1st = 0.6826
            _2st = 0.9544
            for k in [0, 1, 2]:
                plt.subplot(3, 1, k + 1)
                plt.ylabel(ylabel=ylabels[k])
                idx = self.column_index[2 + k]
                err = e.errs[idx]
                self.__plot_cdf_pdf(err)
            plt.xlabel("error / m")

    def __plot_vel_error(self, e: ErrorAnalyser):
        if self.column_index[5] > 0 and self.column_index[6] > 0 and self.column_index[7] > 0:
            plt.figure("vel_err")
            plt.subplot(3, 1, 1)
            plt.title("velocity error")
            plt.plot(e.times - e.times[0], e.errs[self.column_index[5]].error, 'g')
            plt.legend(['North error(rms = %5.3f)' % e.errs[self.column_index[5]].rms])
            plt.subplot(3, 1, 2)

            plt.plot(e.times - e.times[0], e.errs[self.column_index[6]].error, 'g')
            plt.legend(['East error(rms = %5.3f)' % e.errs[self.column_index[6]].rms])
            plt.subplot(3, 1, 3)
            plt.plot(e.times - e.times[0], e.errs[self.column_index[7]].error, 'g')
            plt.legend(['North error(rms = %5.3f)' % e.errs[self.column_index[7]].rms])
            plt.xlabel("gps time/s - %f" % e.times[0])
            ''' plot cdf and pdf'''
            plt.figure('vel_pdf_cdf')
            plt.title("Velocity error pdf and cdf")
            _1st = 0.6826
            _2st = 0.9544
            for k in [0, 1, 2]:
                plt.subplot(3, 1, k + 1)
                idx = self.column_index[5 + k]
                err = e.errs[idx]
                self.__plot_cdf_pdf(err)
            plt.xlabel("error /(m/s)")

    def __plot_atti_error(self, e: ErrorAnalyser):
        if self.column_index[8] > 0 and self.column_index[9] > 0 and self.column_index[10] > 0:
            plt.figure("atti_err")
            plt.subplot(3, 1, 1)
            plt.title("Attitude error")
            plt.plot(e.times - e.times[0], e.errs[self.column_index[8]].error, 'g')
            plt.legend(['North error(rms = %5.3f)' % e.errs[self.column_index[8]].rms])
            plt.subplot(3, 1, 2)

            plt.plot(e.times - e.times[0], e.errs[self.column_index[9]].error, 'g')
            plt.legend(['East error(rms = %5.3f)' % e.errs[self.column_index[9]].rms])
            plt.subplot(3, 1, 3)
            plt.plot(e.times - e.times[0], e.errs[self.column_index[10]].error, 'g')
            plt.legend(['North error(rms = %5.3f)' % e.errs[self.column_index[10]].rms])
            plt.xlabel("gps time/s - %f" % e.times[0])
            ''' plot cdf and pdf'''
            plt.figure('atti_pdf_cdf')
            plt.title("Attitude error pdf and cdf")

            for k in [0, 1, 2]:
                plt.subplot(3, 1, k + 1)
                idx = self.column_index[8 + k]
                err = e.errs[idx]
                self.__plot_cdf_pdf(err)
            plt.xlabel("error /(deg/s)")

    def __plot_tables(self, e: ErrorAnalyser):
        # Markdown:
        table = pt.PrettyTable()
        table.field_names = [" ", "x error /m", "y error/m", "z error/m"]
        table.border = True
        table.junction_char = "|"
        table.horizontal_char = '-'
        table.vertical_char = '|'
        fmt = "%.4f"

        header2 = ["position error(m)", "velocity error(m/s)", "attitude error(deg)"]
        for k in range(3):
            indexs = self.column_index[3 * k + 2:3 * k + 5]
            if indexs[0] > 0 and indexs[2] > 0 and indexs[1] > 0:
                table.add_row([header2[k] + " 1-$\\sigma$", fmt % e.errs[indexs[0]].first_sigma,
                               fmt % e.errs[indexs[1]].first_sigma, fmt % e.errs[indexs[2]].first_sigma])
                table.add_row(
                    [header2[k] + " 2-$\\sigma$", fmt % e.errs[indexs[0]].second_sigma,
                     fmt % e.errs[indexs[1]].second_sigma,
                     fmt % e.errs[indexs[2]].second_sigma])

                table.add_row([header2[k] + " rms", fmt % e.errs[indexs[0]].rms, fmt % e.errs[indexs[1]].rms,
                               fmt % e.errs[indexs[2]].rms])
        table.add_row(["2D error" + " 1-$\\sigma$", fmt % e.error_2d.first_sigma, "", ""])
        table.add_row(["2D error" + " 2-$\\sigma$", fmt % e.error_2d.second_sigma, "", ""])
        table.add_row(["2D error" + " RMS$", fmt % e.error_2d.rms, "", ""])
        table.add_row(["3D error" + " 1-$\\sigma$", fmt % e.error_3d.first_sigma, "", ""])
        table.add_row(["3D error" + " 2-$\\sigma$", fmt % e.error_3d.second_sigma, "", ""])
        table.add_row(["3D error" + " RMS $\\sigma$", fmt % e.error_3d.rms, "", ""])
        print(table)
        self.show_log("\n" + str(table))

    def __plot_cdf_pdf(self, err: Error):
        _1st = 0.6826
        _2st = 0.9544
        hist = err.pdf
        plt.plot(err.edges[1:], hist / max(hist), 'g')
        plt.plot(err.edges[1:], err.cdf / err.cdf[-1], 'b')

        plt.plot([err.first_sigma, err.first_sigma], [0, 1], 'y')
        plt.plot([err.edges[1], err.edges[-1]], [_1st, _1st], 'y')
        plt.plot([err.second_sigma, err.second_sigma], [0, 1], 'r')
        plt.plot([err.edges[1], err.edges[-1]], [_2st, _2st], 'r')
        plt.annotate('%.4f' % err.first_sigma,
                     xy=(err.first_sigma, _1st), xytext=(-1, 1),
                     textcoords='offset points')
        plt.annotate('%.4f' % err.second_sigma,
                     xy=(err.second_sigma, _2st), xytext=(-1, 1),
                     textcoords='offset points')
        plt.legend([' pdf', ' cdf'])

    def __plot_2d_error(self, e: ErrorAnalyser):
        if self.column_index[2] > 0 and self.column_index[3] > 0 and self.column_index[4] > 0:
            plt.figure("error_2d")
            plt.plot(e.times, e.error_2d.error)
            plt.xlabel("gps second/s")
            plt.ylabel("2d error /m")
            plt.title("2D-error")
            plt.figure("2D error cdf/pdf")
            self.__plot_cdf_pdf(e.error_2d)
            plt.xlabel("error /m")

    def __plot_3d_error(self, e: ErrorAnalyser):
        if self.column_index[2] > 0 and self.column_index[3] > 0 and self.column_index[4] > 0:
            plt.figure("error_3d")
            plt.plot(e.times, e.error_3d.error)
            plt.xlabel("gps second/s")
            plt.ylabel("3d error /m")
            plt.title("3D-error")
            plt.figure("3D error cdf/pdf")
            self.__plot_cdf_pdf(e.error_3d)
            plt.xlabel("error /m")

    def onBtnBrowserTargetFile(self):
        filepath, suffix = QFileDialog.getOpenFileName()
        self.editTargetFile.setText(filepath)
        self.show_log("target path: %s" % filepath)

    def onBtnBrowserRefFile(self):
        filfilepath, suffix = QFileDialog.getOpenFileName()
        self.editRefFile.setText(filfilepath)
        self.show_log("ref path: %s" % filfilepath)

    def show_log(self, msg):
        stamp = datetime.now().strftime('%H:%M:%S : ')
        item = stamp + msg
        self.listLog.addItem(item)

    def clear_log(self):
        self.listLog.clear()

    def closeEvent(self, a0: QCloseEvent) -> None:
        plt.close('all')  # 关闭绘图


if __name__ == '__main__':
    import sys

    app = QApplication(sys.argv)
    errorAnalyseWindow = ErrorAnalyseWindow()
    errorAnalyseWindow.show()
    sys.exit(app.exec_())
